/* gm_serial.c -- Arquivo core do gearserial
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <time.h>
#include <string.h>
#include <math.h>

#include "gm_utils.h"
#include "gm_serial.h"

#define ROBOT_RADIUS 0.09
#define MAX_VEL_MS 2
#define MIN_VEL_PWM 0

/* Static */
static void gm_serial_update_checksun( gm_serial_t * self );
static void gm_serial_send_command( gm_serial_t * self );
static void gm_serial_set_conf( gm_serial_t * self, const char conf );
static void gm_serial_motor_set_direction( unsigned char * conf, int motor, int invert );
static int gm_serial_get_ms_as_pwm( const float vel_ms );
    
/** Cria um novo objeto Core que é renponsável por passar os comandos
 * para a serial.
 *
 * @param *end Endereço do robô. Deve ser um vetor de tamanho 3.
 * 
 * @param *serial_path Caminho para a serial do computador.
 *
 * @return Retorna um novo objeto Core.
 */
gm_serial_t *
gm_serial_new( const char *end,
	       const char * serial_path )
{
    gm_serial_t * self = NULL;

    self = XMALLOC( gm_serial_t, 1 );

    self->path = XMALLOC( char, strlen(serial_path) + 1);
    strcpy( self->path, serial_path );

    self->fd = open(self->path, O_RDWR | O_NOCTTY | O_NDELAY);
    if (self->fd == -1)
    {
	xerror_error("Naum consegue abrir a serial");
	return NULL;
    }

    self->speed = B9600;
    
    tcgetattr (self->fd, &(self->options));

    /* velocidade */
    cfsetspeed (&(self->options), self->speed);

    /* 8 bits por byte */
    self->options.c_cflag |= CS8;

    tcsetattr (self->fd, TCSANOW, &(self->options) );

    self->command_size = 9;
    self->command = XMALLOC( unsigned char, self->command_size );

    /* DEFINIÇÃO DA PALAVRA DO PROTOCOLO */
    /* BYTE 1 - primeiro byte do endereço */
    self->command[0] = end[0];

    /* BYTE 2 - segundo byte do endereço */
    self->command[1] = end[1];

    /* BYTE 3 - terceiro byte do endereço */
    self->command[2] = end[2];

    /* BYTE 4 - byte de configuração, que engloba o chute, o rolinho e os sentidos dos motores */
    self->conf = &self->command[3];
    self->command[3] = 0x01;

    /* BYTE 5 - velocidade do motor 1 */
    self->motor_1 = &self->command[4];
    self->command[4] = 0x00;

    /* BYTE 6 - velocidade do motor 2 */
    self->motor_2 = &self->command[5];
    self->command[5] = 0x00;

    /* BYTE 7 - velocidade do motor 3 */
    self->motor_3 = &self->command[6];
    self->command[6] = 0x00;

    /* BYTE 8 - checksum high */
    self->command[7] = 0x00;

    /* BYTE 9 - checksum low */
    self->command[8] = 0x00;
    /* FIM DA DEFINIÇÃO DA PALAVRA DO PROTOCOLO */

    self->mascara1 = 0x000000FF;
    self->mascara2 = 0x0000FF00;

    self->delay.tv_sec = 0;
    self->delay.tv_nsec = 10000;

    self->command_repeat = 1;

    /* Criando matrizes de conversão */
    self->matrix_conv = gm_matrix_new(3,3);
    gm_matrix_set( self->matrix_conv, 0, 0, 1.f );
    gm_matrix_set( self->matrix_conv, 0, 1, 0.5f );
    gm_matrix_set( self->matrix_conv, 0, 2, ROBOT_RADIUS );

    gm_matrix_set( self->matrix_conv, 1, 0, -1.f );
    gm_matrix_set( self->matrix_conv, 1, 1, 0.5f );
    gm_matrix_set( self->matrix_conv, 1, 2, ROBOT_RADIUS );

    gm_matrix_set( self->matrix_conv, 2, 0, 0.f );
    gm_matrix_set( self->matrix_conv, 2, 1, -1.f );
    gm_matrix_set( self->matrix_conv, 2, 2, ROBOT_RADIUS );

    /* Criando matriz que irá conter informações de velocidades
     * relativas vx, vy e va */
    self->matrix_vels = gm_matrix_new( 3, 1 );

    /* Criando matriz que irá conter as velocidade de cada um dos 3
     * motores */
    self->matrix_motor_vels = gm_matrix_new( 3, 1 );
	
    return self;
}

/** Seta a velocidade frontal, lateral e de rotação do robô.
 *
 * Esta função tem uma característica importante que a difere de uma
 * função usual. Os valores passados para vx, vy e va devem estar no
 * intervalo de 0 a 255. Isto deve-se pelo fato dos valores seram na
 * realidade os valores de PWM dos motores.
 *
 * A explicação para estes valores serem os valores de PWM é que o
 * Bala e o SuperBala não possuem mecanismos para aferir a velocidade
 * de suas rodas.
 *
 * @param *self Objeto gm_serial_t.
 * 
 * @param vx Velocidade frontal.
 *
 * @param vy Velocidade lateral.
 *
 * @param va Velocidade de rotação.
 */
void
gm_serial_robot_set_vel( gm_serial_t * self, float vx, float vy, float va )
{
    int v1 = 0;
    int v2 = 0;
    int v3 = 0;
    
    /* Atualiza matriz que será multiplicada */
    gm_matrix_set( self->matrix_vels, 0, 0, vx );
    gm_matrix_set( self->matrix_vels, 1, 0, vy );
    gm_matrix_set( self->matrix_vels, 2, 0, va );

    gm_matrix_mult( self->matrix_conv,
		    self->matrix_vels,
		    self->matrix_motor_vels );

    /* Atualiza velocidade dos motores */
    v1 = gm_serial_get_ms_as_pwm(gm_matrix_get( self->matrix_motor_vels, 0, 0 ));
    v2 = gm_serial_get_ms_as_pwm(gm_matrix_get( self->matrix_motor_vels, 1, 0 ));
    v3 = gm_serial_get_ms_as_pwm(gm_matrix_get( self->matrix_motor_vels, 2, 0 ));

    gm_matrix_clean( self->matrix_motor_vels );

    /* Motor 1 */
    if( v1 < 0 )
    {
      *(self->motor_1) = (unsigned char) v1 * -1;
	gm_serial_motor_set_direction( self->conf, 1, 1 );
    }
    else
    {

	*(self->motor_1) = (unsigned char) v1;
	gm_serial_motor_set_direction( self->conf, 1, 0 );
    }

    /* Motor 2 */
    if( v2 < 0 )
    {
      *(self->motor_2) = (unsigned char) v2 * -1;
	gm_serial_motor_set_direction( self->conf, 2, 1 );
    }
    else
    {
      *(self->motor_2) = (unsigned char) v2;
	gm_serial_motor_set_direction( self->conf, 2, 0 );
    }

    /* Motor 3 */
    if( v3 < 0 )
    {
      *(self->motor_3) = (unsigned char) v3 * -1;
	gm_serial_motor_set_direction( self->conf, 3, 1 );
    }
    else
    {
      *(self->motor_3) = (unsigned char) v3;
	gm_serial_motor_set_direction( self->conf, 3, 0 );
    }

    gm_serial_update_checksun( self );
    gm_serial_send_command( self );
}

static int
gm_serial_get_ms_as_pwm( const float vel_ms )
{
  int sinal = 1;
  int pwm = 0;

  if( vel_ms < 0 )
    sinal = -1;

  pwm = sinal * (((255 * fabs(vel_ms))/MAX_VEL_MS) + MIN_VEL_PWM);

  return pwm;
  
}

/** Seta a direcao do motor alterando a palavra de configuração.
 *
 * @param[out] *conf Ponteiro para a palavra de configuração.
 *
 * @param motor Número do motor. Motor 1, 2 ou 3.
 *
 * @param invert Booleano. 1 seta direção invertida, 0 seta pra
 * normal.
 */
static void
gm_serial_motor_set_direction( unsigned char * conf, int motor, int invert )
{
    switch( motor )
    {
    case 3:
    {
	if( invert )
	{
	    *(conf) |= 0x01;
	}
	else
	{
	    *(conf) &= 0xFE;
	}
	break;
    }
    case 2:
    {
	if( invert )
	{
	    *(conf) |= 0x02;
	}
	else
	{

	    *(conf) &= 0xFD;
	}
	break;
    }
    case 1:
    {
	if( invert )
	{
	    *(conf) |= 0x04;
	}
	else
	{
	    *(conf) &= 0xFB;
	}
	break;
    }
    }
}

/** Liga ou desliga o sistema de drible.
 *
 * @param *self Objeto core.
 *
 * @param enable Variável TRUE ou FALSE que liga e desliga o sistema
 * de drible.
 */
void
gm_serial_robot_dribble_enable( gm_serial_t * self, int enable )
{
    int repeat = 0;

    if( enable )
	*(self->conf) ^= 0x80;
    else
	*(self->conf) &= 0x7F;

    gm_serial_update_checksun( self );

    repeat = self->command_repeat;
    self->command_repeat = 1;
    gm_serial_send_command( self );
    self->command_repeat = repeat;
}

/** Chuta.
 *
 * @param *self Objeto Core.
 */
void
gm_serial_robot_kick( gm_serial_t * self, const int nivel )
{
//    unsigned char saved_conf = 0;
    int repeat = 0;
   
    *(self->conf) ^= 0x40;

    gm_serial_update_checksun( self );

    repeat = self->command_repeat;
    self->command_repeat = 1;
    gm_serial_send_command( self );
    self->command_repeat = repeat;
    
    *(self->conf) &= 0xBF;
}

/** Move o robo para frente acionando as duas rodas laterias.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gm_serial_set_robot_forward( gm_serial_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gm_serial_set_conf( self, 0x02);

    gm_serial_update_checksun( self );
    gm_serial_send_command( self );
}

/** Move o robo para tras acionando as duas rodas laterias.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gm_serial_set_robot_backward( gm_serial_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gm_serial_set_conf( self, 0x04);

    gm_serial_update_checksun( self );
    gm_serial_send_command( self );
}

/** Gira o robô para a esquerda.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gm_serial_set_robot_turn_left( gm_serial_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gm_serial_set_conf( self, 0x00);

    gm_serial_update_checksun( self );
    gm_serial_send_command( self );
}

/** Gira o robô para a direita.
 *
 * @param *self Objeto Core.
 *
 * @param speed Velocidade em que o robô irá andar.
 */
void
gm_serial_set_robot_turn_right( gm_serial_t * self, const unsigned char speed )
{
    *(self->motor_1) = speed;
    *(self->motor_2) = speed;
    *(self->motor_3) = 0x00;
    gm_serial_set_conf( self, 0x07);

    gm_serial_update_checksun( self );
    gm_serial_send_command( self );
}

/** Para o robô.
 *
 * @param *self Objeto Core.
 */
void
gm_serial_set_robot_stop( gm_serial_t * self )
{
    *(self->motor_1) = 0x00;
    *(self->motor_2) = 0x00;
    *(self->motor_3) = 0x00;
    gm_serial_set_conf( self, 0x00 );

    gm_serial_update_checksun( self );
    gm_serial_send_command( self );
}

static void
gm_serial_set_conf( gm_serial_t * self, const char conf )
{
    *(self->conf) &= 0xF0;
    *(self->conf) ^= conf;
}

/** Envia o comando pela serial.
 *
 * @param *self Ponteiro para objeto gm_serial_t.
 */
static void
gm_serial_send_command( gm_serial_t * self )
{
    int i = 0;

    for (i = 0; i < self->command_repeat; i++)
    {
	if( write (self->fd, self->command, self->command_size) < 0)
	    xerror_error("Nao consegui escrever na serial");	
	nanosleep (&(self->delay), &(self->sobrando) );
    }

}

/** Atualiza o checksun na estrutura
 *
 * @param *self Ponteiro para objeto gm_serial_t.
 */
static void
gm_serial_update_checksun( gm_serial_t * self )
{
    unsigned int total = 0;
    unsigned int roda = 0;
    int k = 0;
    
    /* Checksum */
    for (k = 0; k < self->command_size - 2; k++)
    {
	total += self->command[k];
    }
 
    roda = total;
    roda >>= 8;
    self->command[7] = (unsigned char) roda;
    self->command[8] = (unsigned char) total;
}

/** Destroi um objeto Core.
 *
 * @param *self O objeto.
 */
void
gm_serial_destroy( gm_serial_t * self )
{
    XFREE( self->path );
    XFREE( self->command );

    close( self->fd );
    
    XFREE( self );
}
